/**
 * @file peripheral_event.h
 * @author haibo
 * @date Dec 26, 2023
 * @brief Peripheral Event
 * Contact: haibo.yang@lgmgim.cn
 */

#pragma once
#include <agv_msgs/msg/agv_event.hpp>
#include <agv_peripheral_controller/toolKit.h>
#include <rclcpp/rclcpp.hpp>
#include <functional>
#include <chrono>

#define AGVEvent_TO_STREAM(stream, event) \
    stream << "AGVEvent:\n"; \
    stream << "  header:\n"; \
    stream << "    stamp: " << event.header.stamp.sec << "." << std::setfill('0') << std::setw(9) << event.header.stamp.nanosec << "\n"; \
    stream << "    frame_id: " << event.header.frame_id << "\n"; \
    stream << "  type: " << static_cast<int>(event.type) << "\n"; \
    stream << "  action: " << static_cast<int>(event.action) << "\n"; \
    stream << "  ackreq: " << static_cast<int>(event.ackreq) << "\n"; \
    stream << "  eventtime: " << std::fixed << std::setprecision(3) << event.eventtime << "s\n"; \
    stream << "  vellimit: " << event.vellimit << "m/s\n"; \
    stream << "  ecode: " << event.ecode << "\n"; \
    stream << "  param1: " << event.param1 << "\n"; \
    stream << "  param2: " << event.param2 << "\n"; \
    stream << "  param3: " << event.param3 << "\n"; \
    stream << "  param4: " << event.param4 << "\n"; \
    stream << "  description_cn: \"" << event.description_cn << "\"\n"; \
    stream << "  description: \"" << event.description << "\""

inline std::ostream& operator<<(std::ostream& os, const agv_msgs::msg::AGVEvent& event) {
      AGVEvent_TO_STREAM(os, event);
      return os;
    }

class PeripheralEvent {
public:
    PeripheralEvent() {}

    void init(std::shared_ptr<rclcpp::Node> nh) {
        node_ = nh;
        event_pub_ = nh->create_publisher<agv_msgs::msg::AGVEvent>("/agvevent", 10);
        timer_ = nh->create_wall_timer(
            std::chrono::seconds(1), 
            std::bind(&PeripheralEvent::EventPublish, this)
        );
    }

    void addEvent(int ecode, const std::string& message, int param1 = 0, int param2 = 0) {
        agv_msgs::msg::AGVEvent event;
        auto timestamp = node_->now();
        event.header.stamp = timestamp;
        event.header.frame_id = node_->get_name();
        event.ecode = ecode;
        event.description = message;
        event.action = agv_msgs::msg::AGVEvent::ESTOP;
        event.type = agv_msgs::msg::AGVEvent::ALARM;
        event.ackreq = false;
        event.param1 = param1;
        event.param2 = param2;
        event.eventtime = timestamp.seconds();
        
        events_.push_back(event);
        publishEvent(event);
        DEBUG_STREAM_OUT("[Peripheral] Add and publish event:" << event);
    }

    void updateEvent(int ecode, const std::string& message, int param1 = 0, int param2 = 0) {
        auto it = std::find_if(events_.begin(), events_.end(),
            [ecode](const agv_msgs::msg::AGVEvent& event) { 
                return event.ecode == ecode; 
            });

        if (it != events_.end()) {
            auto timestamp = node_->now();
            it->header.stamp = timestamp;
            it->header.frame_id = node_->get_name();
            it->description = message;
            it->param1 = param1;
            it->param2 = param2;
            publishEvent(*it);
            DEBUG_STREAM_OUT_THROTTLE(node_->get_clock(), 1000, "[Peripheral] Update and publish event:" << *it);
        } else {
            addEvent(ecode, message, param1, param2);
        }
    }

    void removeEvent(int ecode) {
        auto new_end = std::remove_if(events_.begin(), events_.end(),
            [ecode](const agv_msgs::msg::AGVEvent& event) {
                if (event.ecode == ecode) {
                    DEBUG_STREAM_OUT("[Peripheral] Delete event:" << event);
                    return true;
                }
                return false;
            });
        events_.erase(new_end, events_.end());
    }

    bool checkEvent(int ecode) {
        return std::any_of(events_.begin(), events_.end(),
            [ecode](const agv_msgs::msg::AGVEvent& event) {
                return event.ecode == ecode;
            });
    }

    void publishEvent(const agv_msgs::msg::AGVEvent& event) {
        event_pub_->publish(event);
    }

    void publishNormal() {
        agv_msgs::msg::AGVEvent event;
        auto timestamp = node_->now();
        event.header.stamp = timestamp;
        event.header.frame_id = node_->get_name();

        event.ecode = agv_msgs::msg::AGVEvent::NONE_EVENT;
        event.description = "No event";
        event.action = agv_msgs::msg::AGVEvent::NONE;
        event.type = agv_msgs::msg::AGVEvent::INFO;
        event_pub_->publish(event);
        DEBUG_STREAM_OUT("[Peripheral] Publish normal");
    }

    void EventPublish() {
        DEBUG_STREAM_OUT("[Peripheral] event size:" << events_.size());
        if (events_.empty()) {
            publishNormal();
        }
    }

private:
    std::shared_ptr<rclcpp::Node> node_;
    rclcpp::Publisher<agv_msgs::msg::AGVEvent>::SharedPtr event_pub_;
    rclcpp::TimerBase::SharedPtr timer_;
    std::vector<agv_msgs::msg::AGVEvent> events_;
};